#include "CameraSensor.h"
#include <stdlib.h>
// #include <AR/gsub.h>
#include <AR/video.h>
#include <ooputils/ConfigManager.h>


using namespace std;
using namespace OOPUtils;


// char			*vconf = "Data\\WDM_camera_flipV.xml";
// char           *cparam_name    = "Data/camera_para.dat";


CameraSensor::CameraSensor(const OOPString& configPath) : thresh (100),
		_basePattern (NULL), _fingerPattern (NULL)
{
	ARParam  wparam, cparam;

	ConfigManager& configMng = ConfigManager::getSingleton ();
	OOPPtr< Config > config = configMng.load (configPath);
	OOPString basePath = configMng.getBasePath ();

	OOPString arConfigPath = basePath + (*config)[L"ArConfig"];
	OOPString arCameraParamPath = basePath + (*config)[L"ArCameraParam"];

	/* open the video path */
	if (arVideoOpen ((char*)arConfigPath.toStdString ().c_str ()) < 0)
		/* TODO: Throw something */;

	/* find the size of the window */
	if (arVideoInqSize(&frameSize[0], &frameSize[1]) < 0)
		/* TODO: Throw something */;

	/* set the initial camera parameters */
	if (arParamLoad (arCameraParamPath.toStdString ().c_str (), 1, &wparam) < 0)
		/* TODO: Throw something */;

	arParamChangeSize (&wparam, frameSize[0], frameSize[1], &cparam);
	arInitCparam (&cparam);
	
	OOPString basePatternPath = basePath + (*config)[L"BasePattern"];
	double basePatternSize = OOPString::convertTo< double > (
			(*config)[L"BasePatternSize"]);
	OOPString fingerPatternPath = basePath + (*config)[L"FingerPattern"];
	double fingerPatternSize = OOPString::convertTo< double > (
			(*config)[L"FingerPatternSize"]);
	_basePattern = new _PatternProperty (basePatternPath.toStdString (),
			1, basePatternSize);
	_fingerPattern = new _PatternProperty (fingerPatternPath.toStdString (),
			2, fingerPatternSize);

	printf ("Image size (x,y) = (%d,%d)\n", frameSize[0], frameSize[1]);
	printf ("*** Camera Parameter ***\n");
	arParamDisp (&cparam);

	arVideoCapStart ();
}


CameraSensor::~CameraSensor(void)
{
	delete _basePattern;
	delete _fingerPattern;
	arVideoCapStop ();
	arVideoClose ();
}

vector< glm::dvec3 > CameraSensor::detectFingerPoints ()
{
	ARUint8* frame;
	ARMarkerInfo* marker_info;
	int marker_num;
	vector< glm::dvec3 > fingerPoints;

	/* grab a vide frame */
	frame = (ARUint8 *)arVideoGetImage ();
	if (frame == NULL) {
		arUtilSleep (2);
		return fingerPoints;
	}

	/* detect the markers in the video frame */
	arDetectMarker (frame, thresh, &marker_info, &marker_num);
	arVideoCapNext ();

	_basePattern->updateProperty (marker_info, marker_num);
	_fingerPattern->updateProperty (marker_info, marker_num);

	if (_basePattern->thisVisible && _fingerPattern->thisVisible)
		fingerPoints.push_back (_fingerPattern->relativeLocFrom (*_basePattern) -
				glm::dvec3 (0.0, -_basePattern->thisSize / 2.0, 0.0));
	return fingerPoints;
}